#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <gnc_functions2.hpp>
#include <ros/time.h>
#include <std_msgs/Bool.h>
#include <cmath>
#include <vector>
#include <fstream>
#include <sstream>

#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
#include <mavros_msgs/CommandLong.h>
ros::ServiceClient servo_client;
bool set_servo(int servo_num, float pwm) {
    mavros_msgs::CommandLong srv;
    srv.request.command = 183;      // MAV_CMD_DO_SET_SERVO
    srv.request.param1 = servo_num; // SERVO号 (1 = output1)
    srv.request.param2 = pwm;       // PWM 值 (1000–2000)
    srv.request.confirmation = 0;
    srv.request.broadcast = false;

    if (servo_client.call(srv)) {
        ROS_INFO("Set servo %d to %.1f us -> success=%d",
                 servo_num, pwm, srv.response.success);
        return srv.response.success;
    } else {
        ROS_ERROR("Failed to call set_servo service!");
        return false;
    }
}


// 全局 ServiceClient

void load_waypoints2_from_csv(const std::string& filename, std::vector<gnc_api_waypoint>& waypoints2) {
    std::ifstream file(filename);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open waypoints2 CSV file: %s", filename.c_str());
        return;
    }

    std::string line;
    std::getline(file, line); // skip header

    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string x_str, y_str, z_str, psi_str;
        if (std::getline(ss, x_str, ',') &&
            std::getline(ss, y_str, ',') &&
            std::getline(ss, z_str, ',') &&
            std::getline(ss, psi_str)) {
            
            gnc_api_waypoint wp;
            wp.x = std::stod(x_str);
            wp.y = std::stod(y_str);
            wp.z = std::stod(z_str);
            wp.psi = std::stod(psi_str);
            waypoints2.push_back(wp);
        }
    }

    file.close();
    ROS_INFO("Loaded %lu waypoints2 from CSV", waypoints2.size());
}


// 当前位置
Eigen::Vector3d t_wb;
// 第二次飞行目标点
std::vector<gnc_api_waypoint> waypoints2;

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    t_wb << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
}

int main(int argc, char** argv)
{
    load_waypoints2_from_csv("/home/jetson/yahboomcar_ws/src/iq_gnc/src/waypoints2.csv", waypoints2);
    //ROS_INFO("waypoints2=%.2f", waypoints2);
    
    ros::init(argc, argv, "gnc_node");
    ros::NodeHandle gnc_node("~");

    // 初始化
    init_publisher_subscriber(gnc_node);
    wait4connect();
    wait4start();
    initialize_local_frame();
    Initialize();
    APMLanding APMLand;

    // 订阅位置
    ros::Subscriber position_sub = gnc_node.subscribe<geometry_msgs::PoseStamped>(
        "/mavros/local_position/pose", 10, pos_cb);
    servo_client = gnc_node.serviceClient<mavros_msgs::CommandLong>("/mavros/cmd/command");

    mavros_setpoint_pos_pub_ = gnc_node.advertise<mavros_msgs::PositionTarget>(
        "/mavros/setpoint_raw/local", 10);

    ros::Rate rate(10);  // 控制频率10Hz
    set_servo(5, 1000);
    // 起飞
    takeoff(3);
    ros::Duration(3).sleep();  // 等待稳定

    // 记录起点
    double home_x = t_wb[0];
    double home_y = t_wb[1];

    // 示例添加两个航点（可由其他模块填入）
    
    int wp2_index = 0;
    bool washing = false;
    ros::Time wash_start;

    while (ros::ok())
    {
        ros::spinOnce();

        if (wp2_index < waypoints2.size()) {
            gnc_api_waypoint wp = waypoints2[wp2_index];
            double dx = wp.x - t_wb[0];
            double dy = wp.y - t_wb[1];
            //double dz = wp.z - t_wb[2];
            double dist = sqrt(dx * dx + dy * dy);

            ROS_INFO("Second Flight: index=%d, dist=%.2f", wp2_index, dist);

            if (dist < 0.5 && !washing) {
                washing = true;
                wash_start = ros::Time::now();
                ROS_INFO("Start cleaning...");
                set_servo(5, 2000);
                ros::Duration(1.0).sleep();  // 延迟 1s
                //set_servo(5, 1000);          // 再收回
                sleep(10);
                set_servo(5, 1000);
            }

            if (washing) {
                if ((ros::Time::now() - wash_start).toSec() > 5.0) {
                    ROS_INFO("Cleaning done. Proceeding to next waypoint.");
                    wp2_index++;
                    washing = false;
                }
            } else {
                set_destination_home(wp.x, wp.y, wp.z);
            }

        } else {
            // 全部清洗完成，回到起点上方 5 米
            set_destination_home(0, 0, 3);
            double dx = t_wb[0] - 0;
            double dy = t_wb[1] - 0;
            double dz = t_wb[2] - 3;
            double dist_to_home = sqrt(dx * dx + dy * dy);

            ROS_INFO("Returning home, distance=%.2f", dist_to_home);
            if (dist_to_home < 1.0) {
                ROS_INFO("Reached return point. Initiating landing.");
                land();
                break;
            }
        }

        rate.sleep();
    }

    ros::shutdown();
    return 0;
}
